close all;
clear; 

try
    % 获取当前文件所在目录
    current_dir = fileparts(mfilename('fullpath'));
    % 将当前目录添加到搜索路径
    addpath(current_dir);
    
    % 检查库是否已加载，如果已加载则先卸载
    if libisloaded('NET_AMC3XER')
        unloadlibrary('NET_AMC3XER');
    end
    
    % 参数设定
    steps_per_rev = 10000;   % 步进电机一圈步数
    gear_ratio = 40;         % 减速比
    destIP = '192.168.1.30'; % 控制卡IP
    axis = 0;                % 电机轴号（假设为0）
    
    % 默认参数定义，确保recover_*赋值不会报错
    default_amp = 90;
    default_speed = 180;
    default_n_cycles = 5;
    default_adc_chan = 0;
    default_supply_current = 0;
    % 直接使用默认参数，变量名全部加recover_前缀，避免与主流程冲突
    recover_amp = default_amp;
    recover_speed = default_speed;
    recover_n_cycles = default_n_cycles;
    recover_adc_chan = default_adc_chan;
    recover_supply_current = default_supply_current;
    
    % 打印参数设置
    fprintf('参数设置：\n');
    fprintf('  三角波幅值：%g 度\n', recover_amp);
    fprintf('  三角波速度：%g 度/秒\n', recover_speed);
    fprintf('  三角波周期数：%d\n', recover_n_cycles);
    fprintf('  ADC通道：%d\n', recover_adc_chan);
    fprintf('  供电电流：%g A\n', recover_supply_current);
    
    % 验证输入
    if recover_adc_chan < 0 || recover_adc_chan > 7
        error('ADC通道必须在0-7之间');
    end
    
    % 三角波参数
    sample_interval = 0.001; % 采样间隔(s)，1000Hz
    ui_update_interval = 0.05; % UI每50ms刷新一次
    last_ui_update_time = tic;
    
    % 角度转步数/步数转角度
    angle2step = @(angle) round(angle * steps_per_rev * gear_ratio / 360);
    step2angle = @(step) step * 360 / (steps_per_rev * gear_ratio);
    
    % 初始化库和设备
    fprintf('正在加载驱动库...\n');
    % 加载库
    loadlibrary('NET_AMC3XER','NET_AMC3XER.h');
    
    % 初始化设备
    fprintf('正在初始化设备...\n');
    init_result = calllib('NET_AMC3XER', 'SOCKET_init');
    if init_result ~= 0
        error('设备初始化失败，错误代码：%d', init_result);
    end
    
    % 设置轴
    axis_result = calllib('NET_AMC3XER', 'Set_Axs', destIP, axis, 1, 0, 0, 0);
    if axis_result ~= 0
        error('轴设置失败，错误代码：%d', axis_result);
    end
    
    % 编码器初始化（4x模式，Z信号不复位，复位为0，enable先0后1）
    calllib('NET_AMC3XER', 'Set_Encorder', destIP, int32(axis), int32(3), int32(0), int32(0), int32(0), int32(0));
    calllib('NET_AMC3XER', 'Set_Encorder', destIP, int32(axis), int32(3), int32(0), int32(0), int32(0), int32(1));
    
    % 数据存储
    max_samples = 10000 * recover_n_cycles * 2; % 预分配足够空间
    enc_data = zeros(max_samples,1);
    adc_data = zeros(max_samples,1);
    time_data = zeros(max_samples,1);
    enc_data_offset = zeros(max_samples,1); % 新增：记录相对初始值
    sample_idx = 1;
    
    fprintf('开始三角波运动与数据采集...\n');
    current_angle = 0;
    enc_data0 = [];
    t_start = tic;
    for half_cycle = 1:(recover_n_cycles*2)
        % 判断奇偶半周期
        if mod(half_cycle,2)==1
            target_angle = recover_amp;
        else
            target_angle = 0;
        end
        delta_angle = target_angle - current_angle;
        delta_step = angle2step(delta_angle);
        if delta_step >= 0
            dir = 0;
        else
            dir = 1;
        end
        length_move = abs(delta_step);
        
        % DeltMov参数设置
        curve = 1;           % S曲线加减速
        outmod = 0;         % 脉冲输出规格
        vo = 1000;          % 初始速度 1000 PPS
        vt = min(250000, abs(recover_speed * steps_per_rev * gear_ratio / 360)); % 运行速度，根据输入的度/秒换算为PPS
        start_dec = 0;      % 自动设定减速位置
        acc_time = 10;     % 加速时间 100ms
        dec_time = 10;     % 减速时间 100ms
        sd_en = 0;          % 不使用手动减速
        wait_sync = 0;      % 立即启动
        
        if length_move > 0
            mov_result = calllib('NET_AMC3XER', 'DeltMov', destIP, axis, curve, dir, ...
                outmod, vo, vt, length_move, start_dec, acc_time, dec_time, sd_en, wait_sync);
            if mov_result == -1
                error('电机运动失败：通信失败，需要重新打开设备');
            end
        end
        
        % 运动监控与实时采样
        while true
            % 读取编码器（Read_Position）
            pos_val = uint32(0);
            runstate = uint8(0);
            iostate = uint8(0);
            cemg = uint8(0);
            pos_ptr = libpointer('uint32Ptr', pos_val);
            runstate_ptr = libpointer('uint8Ptr', runstate);
            iostate_ptr = libpointer('uint8Ptr', iostate);
            cemg_ptr = libpointer('uint8Ptr', cemg);
            enc_result = calllib('NET_AMC3XER', 'Read_Position', destIP, uint32(axis), pos_ptr, runstate_ptr, iostate_ptr, cemg_ptr);
            if enc_result == -1
                error('编码器读取失败，通信错误');
            end
            enc_val = bitand(pos_ptr.Value, hex2dec('FFFFFF'));
            if isempty(enc_data0)
                enc_data0 = enc_val; % 记录初始值
            end
            enc_data_offset(sample_idx) = enc_val - enc_data0;
            
            % 读取ADC
            adc_val = single(0);
            adc_ptr = libpointer('singlePtr', adc_val);
            adc_result = calllib('NET_AMC3XER', 'ADSingle', destIP, recover_adc_chan, adc_ptr);
            if adc_result ~= 0
                error('ADC读取失败，错误代码：%d', adc_result);
            end
            adc_data(sample_idx) = adc_ptr.Value;
            
            % 记录时间
            time_data(sample_idx) = toc(t_start);
            
            % 检查RunState
            if runstate_ptr.Value == 0
                break;
            end
            sample_idx = sample_idx + 1;
            pause(sample_interval);
        end
        current_angle = target_angle;
        % 显示进度
        fprintf('完成进度：%.1f%%\n', half_cycle/(recover_n_cycles*2)*100);
        pause(0.5);
        
    end
    
    % 截取有效数据
    enc_data = enc_data(1:sample_idx-1);
    adc_data = adc_data(1:sample_idx-1);
    time_data = time_data(1:sample_idx-1);
    enc_data_offset = enc_data_offset(1:sample_idx-1);
    
    
catch ME
    % 错误处理
    fprintf('发生错误：%s\n', ME.message);
    fprintf('错误详细信息：\n');
    disp(ME.getReport);
end

% 清理
try
    if libisloaded('NET_AMC3XER')
        calllib('NET_AMC3XER', 'SOCKET_delete');
        unloadlibrary('NET_AMC3XER');
    end
catch cleanup_error
    fprintf('清理过程中发生错误：%s\n', cleanup_error.message);
end 